{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib qt5\n",
    "import pandas as pd\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import seaborn as sns\n",
    "import time\n",
    "from PIL import Image\n",
    "from PIL import ImageDraw\n",
    "plt.style.use({'figure.figsize':(10, 10)})"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Velocity_tripod=0.289*40\n",
    "Velocity_quadruped=0.115*40\n",
    "Velocity_onebyone=0.096*40\n",
    "Up_D=np.array([-40,-20,0,20,40])\n",
    "Left_D=np.array([-60,-80,-100,-120])\n",
    "Right_D=np.array([60,80,100,120])\n",
    "Robot_R=40"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Q_table1_gait_state=np.array(['L0R0U0','L0R0U1','L0R0U2','L0R0U3',\n",
    "                'L0R1U0','L0R1U1','L0R1U2','L0R1U3',\n",
    "                'L0R2U0','L0R2U1','L0R2U2','L0R2U3',\n",
    "                'L0R3U0','L0R3U1','L0R3U2','L0R3U3',\n",
    "                'L1R0U0','L1R0U1','L1R0U2','L1R0U3',\n",
    "                'L1R1U0','L1R1U1','L1R1U2','L1R1U3',\n",
    "                'L1R2U0','L1R2U1','L1R2U2','L1R2U3',\n",
    "                'L1R3U0','L1R3U1','L1R3U2','L1R3U3',\n",
    "                'L2R0U0','L2R0U1','L2R0U2','L2R0U3',\n",
    "                'L2R1U0','L2R1U1','L2R1U2','L2R1U3',\n",
    "                'L2R2U0','L2R2U1','L2R2U2','L2R2U3',\n",
    "                'L2R3U0','L2R3U1','L2R3U2','L2R3U3',\n",
    "                'L3R0U0','L3R0U1','L3R0U2','L3R0U3',\n",
    "                'L3R1U0','L3R1U1','L3R1U2','L3R1U3',\n",
    "                'L3R2U0','L3R2U1','L3R2U2','L3R2U3',\n",
    "                'L3R3U0','L3R3U1','L3R3U2','L3R3U3'])\n",
    "Q_table2_gait_state=np.array(['L60D0/30/H0','L180D0/30/H0','L250D0/30/H0','L300D0/30/H0',\n",
    "'L60D30/60/H0','L180D30/60/H0','L250D30/60/H0','L300D30/60/H0',\n",
    "'L60D60/90/H0','L180D60/90/H0','L250D60/90/H0','L300D60/90/H0',\n",
    "'L60D90/120/H0','L180D90/120/H0','L250D90/120/H0','L300D90/120/H0',\n",
    "'L60D120/150/H0','L180D120/150/H0','L250D120/150/H0','L300D120/150/H0',\n",
    "'L60D150/180/H0','L180D150/180/H0','L250D150/180/H0','L300D150/180/H0',\n",
    "'L60D180/210/H0','L180D180/210/H0','L250D180/210/H0','L300D180/210/H0',\n",
    "'L60D210/240/H0','L180D210/240/H0','L250D210/240/H0','L300D210/240/H0',\n",
    "'L60D240/270/H0','L180D240/270/H0','L250D240/270/H0','L300D240/270/H0',\n",
    "'L60D270/300/H0','L180D270/300/H0','L250D270/300/H0','L300D270/300/H0',\n",
    "'L60D300/330/H0','L180D300/330/H0','L250D300/330/H0','L300D300/330/H0',\n",
    "'L60D330/360/H0','L180D330/360/H0','L250D330/360/H0','L300D330/360/H0',\n",
    "'L60D0/30/H1','L180D0/30/H1','L250D0/30/H1','L300D0/30/H1',\n",
    "'L60D30/60/H1','L180D30/60/H1','L250D30/60/H1','L300D30/60/H1',\n",
    "'L60D60/90/H1','L180D60/90/H1','L250D60/90/H1','L300D60/90/H1',\n",
    "'L60D90/120/H1','L180D90/120/H1','L250D90/120/H1','L300D90/120/H1',\n",
    "'L60D120/150/H1','L180D120/150/H1','L250D120/150/H1','L300D120/150/H1',\n",
    "'L60D150/180/H1','L180D150/180/H1','L250D150/180/H1','L300D150/180/H1',\n",
    "'L60D180/210/H1','L180D180/210/H1','L250D180/210/H1','L300D180/210/H1',\n",
    "'L60D210/240/H1','L180D210/240/H1','L250D210/240/H1','L300D210/240/H1',\n",
    "'L60D240/270/H1','L180D240/270/H1','L250D240/270/H1','L300D240/270/H1',\n",
    "'L60D270/300/H1','L180D270/300/H1','L250D270/300/H1','L300D270/300/H1',\n",
    "'L60D300/330/H1','L180D300/330/H1','L250D300/330/H1','L300D300/330/H1',\n",
    "'L60D330/360/H1','L180D330/360/H1','L250D330/360/H1','L300D330/360/H1',\n",
    "'L60D0/30/H2','L180D0/30/H2','L250D0/30/H2','L300D0/30/H2',\n",
    "'L60D30/60/H2','L180D30/60/H2','L250D30/60/H2','L300D30/60/H2',\n",
    "'L60D60/90/H2','L180D60/90/H2','L250D60/90/H2','L300D60/90/H2',\n",
    "'L60D90/120/H2','L180D90/120/H2','L250D90/120/H2','L300D90/120/H2',\n",
    "'L60D120/150/H2','L180D120/150/H2','L250D120/150/H2','L300D120/150/H2',\n",
    "'L60D150/180/H2','L180D150/180/H2','L250D150/180/H2','L300D150/180/H2',\n",
    "'L60D180/210/H2','L180D180/210/H2','L250D180/210/H2','L300D180/210/H2',\n",
    "'L60D210/240/H2','L180D210/240/H2','L250D210/240/H2','L300D210/240/H2',\n",
    "'L60D240/270/H2','L180D240/270/H2','L250D240/270/H2','L300D240/270/H2',\n",
    "'L60D270/300/H2','L180D270/300/H2','L250D270/300/H2','L300D270/300/H2',\n",
    "'L60D300/330/H2','L180D300/330/H2','L250D300/330/H2','L300D300/330/H2',\n",
    "'L60D330/360/H2','L180D330/360/H2','L250D330/360/H2','L300D330/360/H2'])\n",
    "Q_table2_gait_action=['FrontH0','FrontH1','FrontH2','Left_60D','Right_60D']\n",
    "Q_table1_gait_action=['Ahead','Back','Left','Right']"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Q_table1_gait=np.loadtxt(r\"E:\\Graduate\\python\\Q_Table_notgait_finish\\Q_Table1_notgait\\Q_Table1_notgait_1.txt\")\n",
    "Q_table1_gait_show=pd.DataFrame(Q_table1_gait,columns=Q_table1_gait_action,index=Q_table1_gait_state)\n",
    "Q_table1_gait_show['Final_action_index']=np.argmax(Q_table1_gait,axis=1)\n",
    "print('********************** first Q table **********************')\n",
    "# print(Q_table1_gait_show)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Q_table2_gait=pd.DataFrame([])\n",
    "for i in range(17):\n",
    "    Qtable2_names=((r'E:\\Graduate\\python\\Q_Tablegait6map_finish\\Q_Table2_gait\\Table2gait6map')+str(i)+'.txt')\n",
    "    Load_table=np.loadtxt(Qtable2_names)\n",
    "    Q_table2_gait[('Max')+str(i)]=Load_table\n",
    "f=lambda x:np.argmax(np.bincount(x))\n",
    "Q_table2_gait=pd.DataFrame(Q_table2_gait, dtype=np.int8)\n",
    "Q_table2_gait['Final_action']=Q_table2_gait.apply(f,axis=1)\n",
    "Q_table2_gait.index=Q_table2_gait_state\n",
    "print('********************** second Q table **********************')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Draw_map_test(Destination_x,Destination_y):\n",
    "    im=Image.new('RGB',size=(2000,2000),color=(0,0,0))\n",
    "    draw=ImageDraw.Draw(im,mode='RGB')\n",
    "    \n",
    "    draw.rectangle((40,40,1960,1960),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((900,1300,1500,1800),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((500,250,800,500),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((1100,300,1800,700),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((100,1000,500,1700),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((750,1000,1300,1300),(202,235,216),(202,235,216))\n",
    "    \n",
    "    \n",
    "    draw.rectangle((1500,1000,1650,1150),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((400,700,550,850),(0,0,0), (0,0,0)) \n",
    "    draw.rectangle((200,300,350,450),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((1500,500,1550,650),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((1200,1400,1350,1550),(0,0,0), (0,0,0)) \n",
    "    draw.rectangle((700,1200,850,1350),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((300,1600,450,1750),(0,0,0), (0,0,0)) \n",
    "    draw.rectangle((100,1100,250,1250),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((1100,250,1250,400),(0,0,0), (0,0,0)) \n",
    "    draw.polygon((900, 1070,1120, 1000,1150, 1100, 1100,1090,1050, 1200), (0,0,0), (0,0,0))\n",
    "    draw.pieslice((750, 1700, 900, 1850), 0,180,(0,0,0),(0,0,0))\n",
    "    draw.ellipse((900,550,1050,700),(0,0,0), (0,0,0))\n",
    "    draw.ellipse((650,100,750,200),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((Destination_x-20,Destination_y-20,Destination_x+20,Destination_y+20),(218,112,214), (218,112,214))\n",
    "    return im\n",
    "im1=Draw_map_test(1000,500)\n",
    "plt.imshow(im1)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Set_destination(x,y):\n",
    "    return x,y\n",
    "\n",
    "def Start_point(x,y,angle):\n",
    "    return x,y,angle"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Scan_1(Current_x,Current_y,Angle,im):\n",
    "    Dis_level=0\n",
    "    Obs_dis=Robot_R\n",
    "    Obs_dis_x=Current_x+Obs_dis*np.cos(Angle/180*np.pi)\n",
    "    Obs_dis_y=Current_y+Obs_dis*np.sin(Angle/180*np.pi)\n",
    "    while(im.getpixel((Obs_dis_x,Obs_dis_y))!=(0,0,0) and Obs_dis<250):#getpixiel obtains the degree of Gray Scale\n",
    "        Obs_dis+=5  #Search interval, can be changed\n",
    "        Obs_dis_x=Current_x+Obs_dis*np.cos(Angle/180*np.pi)\n",
    "        Obs_dis_y=Current_y+Obs_dis*np.sin(Angle/180*np.pi)\n",
    "    if 0<=Obs_dis<90:\n",
    "        dis_level=0 #Nearear\n",
    "    elif 90<=Obs_dis<170:\n",
    "        dis_level=1 #Near\n",
    "    elif 170<=Obs_dis<250:\n",
    "        dis_level=2 #Medium\n",
    "    else:\n",
    "        dis_level=3 #Far        \n",
    "    return dis_level,Obs_dis\n",
    "\n",
    "def Direction_min_level_1(Degree,Current_x,Current_y,Rotbot_angle,im):\n",
    "    Level=[]\n",
    "    Dis=[]\n",
    "    Degree=Degree+Rotbot_angle\n",
    "    for i in Degree:\n",
    "        Level.append((Scan_1(Current_x,Current_y,i,im))[0])\n",
    "        Dis.append((Scan_1(Current_x,Current_y,i,im))[1])\n",
    "    return min(Level),min(Dis)\n",
    "\n",
    "def Output_state_index_1(Left_min,Right_min,Up_min):\n",
    "    LRU=[]\n",
    "    LRU.append(Left_min)\n",
    "    LRU.append(Right_min)\n",
    "    LRU.append(Up_min)\n",
    "    return LRU[0]*16+LRU[1]*4+LRU[2]\n",
    "\n",
    "def Is_Crash_1(Current_x,Current_y):\n",
    "    Crash=False\n",
    "    Degree=[-150,-120,-90,-60,-30,0,30,60,90,120,150,180]\n",
    "    Detect_distance=np.arange(0,50,5)\n",
    "    for i in Detect_distance:\n",
    "        for j in Degree:\n",
    "            x=Current_x+i*np.cos(j/180*np.pi)\n",
    "            y=Current_y+i*np.sin(j/180*np.pi)\n",
    "            if (im.getpixel((x,y)))==0:\n",
    "                Crash=True\n",
    "                break\n",
    "        if Crash==True:\n",
    "                break\n",
    "    return Crash"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Robot_destination_2(Destination_x,Destination_y,Robot_x,Robot_y,Robot_a):\n",
    "    Delta_x=Destination_x-Robot_x\n",
    "    Delta_y=Destination_y-Robot_y\n",
    "    Distance=np.sqrt(Delta_x*Delta_x+Delta_y*Delta_y)\n",
    "    Direct_angle=(np.arctan(Delta_y/Delta_x))*180/np.pi\n",
    "    if Delta_x<0:\n",
    "        Direct_angle=Direct_angle+180\n",
    "    True_angle=(Robot_a-Direct_angle)%360\n",
    "    if True_angle>180:\n",
    "        return 360-True_angle,'right',Distance\n",
    "    else:\n",
    "        return True_angle,'left',Distance\n",
    "    \n",
    "def Is_arrive_2(Current_x,Current_y,im):\n",
    "    Arrive=False\n",
    "    Crash=False\n",
    "    Degree=[-150,-120,-90,-60,-30,0,30,60,90,120,150,180]\n",
    "    Dis=np.arange(0,30,5)\n",
    "    for i in Dis:\n",
    "        for j in Degree:\n",
    "            x=Current_x+i*np.cos(j/180*np.pi)\n",
    "            y=Current_y+i*np.sin(j/180*np.pi)\n",
    "            if (im.getpixel((x,y)))==(0,0,0):\n",
    "                Crash=True\n",
    "                break\n",
    "            if (im.getpixel((x,y)))==(255,0,0):\n",
    "                Arrive=True\n",
    "                break\n",
    "        if Arrive==True or Crash==True:\n",
    "                break\n",
    "    return Arrive\n",
    "\n",
    "def Height_level_front_2(Current_x,Current_y,Current_a,im):\n",
    "    Degree=[-30,0,30]\n",
    "    Distance=np.arange(0,30,5)\n",
    "    Max_height=0\n",
    "    for i in Degree:\n",
    "        for j in Distance:\n",
    "            x=Current_x+i*np.cos((j+Current_a)/180*np.pi)\n",
    "            y=Current_y+i*np.sin((j+Current_a)/180*np.pi)\n",
    "            Height_level=0\n",
    "            if im.getpixel((x,y))==(255,255,255):\n",
    "                Height_level=0\n",
    "            elif im.getpixel((x,y))==(202,235,216):\n",
    "                Height_level=1\n",
    "            elif im.getpixel((x,y))==(128,138,135):\n",
    "                Height_level=2\n",
    "            Max_height=max(Height_level,Max_height)\n",
    "            if Max_height==2:\n",
    "                break\n",
    "        if Max_height==2:\n",
    "            break\n",
    "    return Height_level\n",
    "\n",
    "def Height_level_back_2(Current_x,Current_y,Current_a,im):\n",
    "    Degree=[150,180,-150]\n",
    "    Distance=np.arange(0,30,5)\n",
    "    Max_height=0\n",
    "    for i in Degree:\n",
    "        for j in Distance:\n",
    "            x=Current_x+i*np.cos((j+Current_a)/180*np.pi)\n",
    "            y=Current_y+i*np.sin((j+Current_a)/180*np.pi)\n",
    "            Height_level=0\n",
    "            if im.getpixel((x,y))==(255,255,255):\n",
    "                Height_level=0\n",
    "            elif im.getpixel((x,y))==(202,235,216):\n",
    "                Height_level=1\n",
    "            elif im.getpixel((x,y))==(128,138,135):\n",
    "                Height_level=2\n",
    "            Max_height=max(Height_level,Max_height)\n",
    "            if Max_height==2:\n",
    "                break\n",
    "        if Max_height==2:\n",
    "            break\n",
    "    return Height_level\n",
    "\n",
    "def Output_state_index_2(True_angle,Left_right,Distance,Height):\n",
    "    if 0<=Distance<60:\n",
    "        Dis_level=0\n",
    "    elif 60<=Distance<180:\n",
    "        Dis_level=1\n",
    "    elif 180<=Distance<250:\n",
    "        Dis_level=2\n",
    "    else:\n",
    "        Dis_level=3\n",
    "    Angle_level=True_angle//30\n",
    "    if Left_right=='right':\n",
    "        Le_ri=1\n",
    "        State_number=int(Height*48+Dis_level+24*Le_ri+(5-Angle_level)*4)\n",
    "    elif Left_right=='left':\n",
    "        Le_ri=0\n",
    "        Angle_level=True_angle//30\n",
    "        State_number=int(Height*48+Dis_level+24*Le_ri+Angle_level*4)\n",
    "    return State_number,Dis_level,Angle_level\n",
    "\n",
    "def Height_level_2(Current_x,Current_y,Current_a,im):\n",
    "    Degree=[-30,0,30]\n",
    "    Distance=np.arange(0,30,5)\n",
    "    Max_height=0\n",
    "    for i in Degree:\n",
    "        for j in Distance:\n",
    "            x=Current_x+i*np.cos((j+Current_a)/180*np.pi)\n",
    "            y=Current_y+i*np.sin((j+Current_a)/180*np.pi)\n",
    "            Height_level=0\n",
    "            if im.getpixel((x,y))==(255,255,255):\n",
    "                Height_level=0\n",
    "            elif im.getpixel((x,y))==(202,235,216):\n",
    "                Height_level=1\n",
    "            elif im.getpixel((x,y))==(128,138,135):\n",
    "                Height_level=2\n",
    "            Max_height=max(Height_level,Max_height)\n",
    "            if Max_height==2:\n",
    "                break\n",
    "        if Max_height==2:\n",
    "            break\n",
    "    return Height_level"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Which_table_action(Avoid_action,Arrive_action,Current_x,Current_y,Current_angle,Destination_x,Destination_y,im):\n",
    "    if Avoid_action==0:\n",
    "        Height_level=Height_level_front_2(Current_x,Current_y,Current_angle,im)\n",
    "        if Height_level==0:\n",
    "            Next_avoid_x=Current_x+Velocity_tripod*np.cos(Current_angle/180*np.pi)\n",
    "            Next_avoid_y=Current_y+Velocity_tripod*np.sin(Current_angle/180*np.pi)\n",
    "            Next_avoid_angle=Current_angle\n",
    "            Next_avoid_action=0\n",
    "        elif Height_level==1:\n",
    "            Next_avoid_x=Current_x+Velocity_quadruped*np.cos(Current_angle/180*np.pi)\n",
    "            Next_avoid_y=Current_y+Velocity_quadruped*np.sin(Current_angle/180*np.pi)\n",
    "            Next_avoid_angle=Current_angle\n",
    "            Next_avoid_action=1\n",
    "        elif Height_level==2:\n",
    "            Next_avoid_x=Current_x+Velocity_onebyone*np.cos(Current_angle/180*np.pi)\n",
    "            Next_avoid_y=Current_y+Velocity_onebyone*np.sin(Current_angle/180*np.pi)\n",
    "            Next_avoid_angle=Current_angle\n",
    "            Next_avoid_action=2\n",
    "    elif Avoid_action==1:\n",
    "        Height_level=Height_level_back_2(Current_x,Current_y,Current_angle,im)\n",
    "        if Height_level==0:\n",
    "            Next_avoid_x=Current_x-Velocity_tripod*np.cos(Current_angle/180*np.pi)\n",
    "            Next_avoid_y=Current_y-Velocity_tripod*np.sin(Current_angle/180*np.pi)\n",
    "            Next_avoid_angle=Current_angle\n",
    "            Next_avoid_action=0\n",
    "        elif Height_level==1:\n",
    "            Next_avoid_x=Current_x-Velocity_quadruped*np.cos(Current_angle/180*np.pi)\n",
    "            Next_avoid_y=Current_y-Velocity_quadruped*np.sin(Current_angle/180*np.pi)\n",
    "            Next_avoid_angle=Current_angle\n",
    "            Next_avoid_action=1\n",
    "        elif Height_level==2:\n",
    "            Next_avoid_x=Current_x-Velocity_onebyone*np.cos(Current_angle/180*np.pi)\n",
    "            Next_avoid_y=Current_y-Velocity_onebyone*np.sin(Current_angle/180*np.pi)\n",
    "            Next_avoid_angle=Current_angle\n",
    "            Next_avoid_action=2\n",
    "    elif Avoid_action==2:\n",
    "        Next_avoid_x=Current_x\n",
    "        Next_avoid_y=Current_y\n",
    "        Next_avoid_angle=Current_angle-45\n",
    "        Next_avoid_action=3\n",
    "    elif Avoid_action==3:\n",
    "        Next_avoid_x=Current_x\n",
    "        Next_avoid_y=Current_y\n",
    "        Next_avoid_angle=Current_angle+45\n",
    "        Next_avoid_action=4\n",
    "    \n",
    "    if Arrive_action==0:\n",
    "        Next_arrive_x=Current_x+Velocity_tripod*np.cos(Current_angle/180*np.pi)\n",
    "        Next_arrive_y=Current_y+Velocity_tripod*np.sin(Current_angle/180*np.pi)\n",
    "        Next_arrive_angle=Current_angle\n",
    "        Next_arrive_action=0\n",
    "    elif Arrive_action==1:\n",
    "        Next_arrive_x=Current_x+Velocity_quadruped*np.cos(Current_angle/180*np.pi)\n",
    "        Next_arrive_y=Current_y+Velocity_quadruped*np.sin(Current_angle/180*np.pi)\n",
    "        Next_arrive_angle=Current_angle\n",
    "        Next_arrive_action=1\n",
    "    elif Arrive_action==2:\n",
    "        Next_arrive_x=Current_x+Velocity_onebyone*np.cos(Current_angle/180*np.pi)\n",
    "        Next_arrive_y=Current_y+Velocity_onebyone*np.sin(Current_angle/180*np.pi)\n",
    "        Next_arrive_angle=Current_angle\n",
    "        Next_arrive_action=2\n",
    "    elif Arrive_action==3:\n",
    "        Next_arrive_x=Current_x\n",
    "        Next_arrive_y=Current_y\n",
    "        Next_arrive_angle=Current_angle-60\n",
    "        Next_arrive_action=3\n",
    "    elif Arrive_action==4:\n",
    "        Next_arrive_x=Current_x\n",
    "        Next_arrive_y=Current_y\n",
    "        Next_arrive_angle=Current_angle+60\n",
    "        Next_arrive_action=4\n",
    "        \n",
    "    Next_left_level_1,Next_left_obstacle_distance_1=Direction_min_level_1(Left_D,Next_avoid_x,Next_avoid_y,Next_avoid_angle,im)\n",
    "    Next_right_level_1,Next_right_obstacle_distance_1=Direction_min_level_1(Right_D,Next_avoid_x,Next_avoid_y,Next_avoid_angle,im)\n",
    "    Next_up_level_1,Next_up_obstacle_disance_1=Direction_min_level_1(Up_D,Next_avoid_x,Next_avoid_y,Next_avoid_angle,im)\n",
    "    Next_min_level_1=min(Next_left_level_1, Next_right_level_1,Next_up_level_1)\n",
    "    \n",
    "    if Next_min_level_1==0:\n",
    "        Next_x=Next_avoid_x\n",
    "        Next_y=Next_avoid_y\n",
    "        Next_angle=Next_avoid_angle\n",
    "#         Next_action='AVOID'+str(Avoid_action)\n",
    "        Next_action=Next_avoid_action\n",
    "        \n",
    "    else:\n",
    "        Next_x=Next_arrive_x\n",
    "        Next_y=Next_arrive_y\n",
    "        Next_angle=Next_arrive_angle\n",
    "        Next_action=Next_arrive_action\n",
    "#         Next_action='ARRIVE'+str(Arrive_action)\n",
    "        \n",
    "    \n",
    "    return Next_x,Next_y,Next_angle,Next_action\n",
    "        \n",
    "        "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Movement_plot(Vec_x_tripod,Vec_y_tripod,Vec_x_quadruped,Vec_y_quadruped,Vec_x_onebyone,Vec_y_onebyone,\n",
    "                  Vec_x_turn,Vec_y_turn,Destination_x,Destination_y):\n",
    "    \n",
    "    im=Image.new('RGB',size=(2000,2000),color=(0,0,0))\n",
    "    draw=ImageDraw.Draw(im,mode='RGB')\n",
    "    \n",
    "    draw.rectangle((40,40,1960,1960),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((900,1300,1500,1800),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((500,250,800,500),(106,90,205),(106,90,205))\n",
    "    draw.rectangle((1100,300,1800,700),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((100,1000,500,1700),(106,90,205),(106,90,205))\n",
    "    draw.rectangle((750,1000,1300,1300),(106,90,205),(106,90,205))\n",
    "    \n",
    "    draw.rectangle((1500,1000,1650,1150),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((400,700,550,850),(0,0,0), (0,0,0)) \n",
    "    draw.rectangle((200,300,350,450),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((1500,500,1550,650),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((1200,1400,1350,1550),(0,0,0), (0,0,0)) \n",
    "    draw.rectangle((700,1200,850,1350),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((300,1600,450,1750),(0,0,0), (0,0,0)) \n",
    "    draw.rectangle((100,1100,250,1250),(0,0,0), (0,0,0)) \n",
    "    draw.ellipse((1100,250,1250,400),(0,0,0), (0,0,0)) \n",
    "    draw.polygon((900, 1070,1120, 1000,1150, 1100, 1100,1090,1050, 1200), (0,0,0), (0,0,0))\n",
    "    draw.pieslice((750, 1700, 900, 1850), 0,180,(0,0,0),(0,0,0))\n",
    "    draw.ellipse((900,550,1050,700),(0,0,0), (0,0,0))\n",
    "    draw.ellipse((650,100,750,200),(0,0,0), (0,0,0)) \n",
    "    draw.rectangle((1700,130,1800,230),(0,0,0),(0,0,0))\n",
    "    draw.ellipse((Destination_x-20,Destination_y-20,Destination_x+20,Destination_y+20),(218,112,214), (218,112,214))\n",
    "    \n",
    "    d=1\n",
    "    for d in range(50):\n",
    "        for i,j,k in zip(Vec_x_tripod[0:],Vec_y_tripod[0:],Vec_time_tripod[0:]):\n",
    "            if 10*(d-1)<k<=10*d:\n",
    "                draw.ellipse((i-40,j-40, i+40,j+40),(69,139,116),(69,139,116))\n",
    "        for i,j,k in zip(Vec_x_quadruped[0:],Vec_y_quadruped[0:],Vec_time_quadruped[0:]):\n",
    "            if 10*(d-1)<k<10*d:\n",
    "                draw.ellipse((i-40,j-40, i+40,j+40),(193,255,193),(193,255,193))\n",
    "        for i,j,k in zip(Vec_x_onebyone[0:],Vec_y_onebyone[0:],Vec_time_onebyone[0:]):\n",
    "            if 10*(d-1)<k<10*d:\n",
    "                draw.ellipse((i-40,j-40, i+40,j+40),(155,205,155),(155,205,155))\n",
    "        for i,j,k in zip(Vec_x_turn[0:],Vec_y_turn[0:],Vec_time_turn[0:]):\n",
    "            if 10*(d-1)<k<10*d:\n",
    "                draw.ellipse((i-40,j-40, i+40,j+40),(84,255,159),(84,255,159))\n",
    "        plt.imshow(im)\n",
    "        plt.savefig((r\"E:\\Graduate\\python\\Q_Tablegait6map_finish\\Pic1\\picture\")+str(d)+\".png\")\n",
    "        d+=1\n",
    "    return im"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "global Vec_x_tripod\n",
    "global Vec_y_tripod\n",
    "global Vec_x_quadruped\n",
    "global Vec_y_quadruped\n",
    "global Vec_x_onebyone\n",
    "global Vec_y_onebyone\n",
    "global Vec_x_turn\n",
    "global Vec_y_turn\n",
    "global Vec_time_tripod\n",
    "global Vec_time_quadruped\n",
    "global Vec_time_onebyone\n",
    "global Vec_time_turn"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    " def Run():   \n",
    "    Destination_x,Destination_y=Set_destination(1500,250)\n",
    "    Current_x,Current_y,Current_angle=Start_point(500,1500,30)\n",
    "    \n",
    "    im=Draw_map_test(Destination_x,Destination_y)\n",
    "    Arrive=False\n",
    "    Crash=False\n",
    "    global Vec_x_tripod\n",
    "    global Vec_y_tripod\n",
    "    global Vec_x_quadruped\n",
    "    global Vec_y_quadruped\n",
    "    global Vec_x_onebyone\n",
    "    global Vec_y_onebyone\n",
    "    global Vec_x_turn\n",
    "    global Vec_y_turn\n",
    "    global Vec_time_tripod\n",
    "    global Vec_time_quadruped\n",
    "    global Vec_time_onebyone\n",
    "    global Vec_time_turn\n",
    "    Vec_x_tripod=[]\n",
    "    Vec_y_tripod=[]\n",
    "    Vec_x_quadruped=[]\n",
    "    Vec_y_quadruped=[]\n",
    "    Vec_x_onebyone=[]\n",
    "    Vec_y_onebyone=[]\n",
    "    Vec_x_turn=[]\n",
    "    Vec_y_turn=[]\n",
    "    Total_step=0\n",
    "    Vec_time_tripod=[]\n",
    "    Vec_time_quadruped=[]\n",
    "    Vec_time_onebyone=[]\n",
    "    Vec_time_turn=[]\n",
    "    \n",
    "    while Arrive!=True and Total_step<600:\n",
    "        Current_left_obstacle_level,_=Direction_min_level_1(Left_D,Current_x,Current_y,Current_angle,im)\n",
    "        Current_left_obstacle_level,_=Direction_min_level_1(Right_D,Current_x,Current_y,Current_angle,im)\n",
    "        Current_up_Obstacle_level,C=Direction_min_level_1(Up_D,Current_x,Current_y,Current_angle,im)\n",
    "        Current_state_avoid=Output_state_index_1(Current_left_obstacle_level,Current_left_obstacle_level\n",
    "                                                 ,Current_up_Obstacle_level)\n",
    "        \n",
    "        Avoid_action=int(Q_table1_gait_show.iloc[Current_state_avoid][4])\n",
    "        \n",
    "        True_angle,Left_right,Distance=Robot_destination_2(Destination_x,Destination_y,Current_x,Current_y,Current_angle)\n",
    "        Height_level=Height_level_2(Current_x,Current_y,Current_angle,im)\n",
    "        Current_state_arrive,Distance_level,Angle_level=Output_state_index_2(True_angle,Left_right,Distance,Height_level)\n",
    "        \n",
    "        Arrive_action=int(Q_table2_gait.iloc[Current_state_arrive][17])\n",
    "        \n",
    "        Next_x,Next_y,Next_angle,Next_action=Which_table_action(Avoid_action,Arrive_action,Current_x,Current_y,Current_angle,Destination_x,Destination_y,im)\n",
    "        \n",
    "        Arrive=Is_arrive_2(Next_x,Next_y,im)\n",
    "        \n",
    "        print('**********************************************************')\n",
    "        print('Current_x=%f    Current_y=%f   Current_angle=%f'%(Current_x,Current_y,Current_angle))\n",
    "        print(Next_action)\n",
    "        print('Next_x=%f    Next_y=%f    Next_angle=%f'%(Next_x,Next_y,Next_angle))\n",
    "#         print('DestinationRobotAngle=%f Robot_current_angle=%d'%((np.arctan((Next_y-Destination_y)/(Next_x-Destination_x)))*180/np.pi,Next_angle%360))\n",
    "        \n",
    "        if Arrive==True:\n",
    "            if Next_action==0:\n",
    "                Vec_x_tripod.append(Current_x)\n",
    "                Vec_y_tripod.append(Current_y)\n",
    "                Vec_time_tripod.append(Total_step)\n",
    "            elif Next_action==1:\n",
    "                Vec_x_quadruped.append(Current_x)\n",
    "                Vec_y_quadruped.append(Current_y)\n",
    "                Vec_time_quadruped.append(Total_step)\n",
    "            elif Next_action==2:\n",
    "                Vec_x_onebyone.append(Current_x)\n",
    "                Vec_y_onebyone.append(Current_y)\n",
    "                Vec_time_onebyone.append(Total_step)\n",
    "            else:\n",
    "                Vec_x_turn.append(Current_x)\n",
    "                Vec_y_turn.append(Current_y) \n",
    "                Vec_time_turn.append(Total_step)\n",
    "\n",
    "            break\n",
    "        else:\n",
    "            if Next_action==0:\n",
    "                Vec_x_tripod.append(Current_x)\n",
    "                Vec_y_tripod.append(Current_y)\n",
    "                Vec_time_tripod.append(Total_step)\n",
    "            elif Next_action==1:\n",
    "                Vec_x_quadruped.append(Current_x)\n",
    "                Vec_y_quadruped.append(Current_y)\n",
    "                Vec_time_quadruped.append(Total_step)\n",
    "            elif Next_action==2:\n",
    "                Vec_x_onebyone.append(Current_x)\n",
    "                Vec_y_onebyone.append(Current_y)\n",
    "                Vec_time_onebyone.append(Total_step)\n",
    "            else:\n",
    "                Vec_x_turn.append(Current_x)\n",
    "                Vec_y_turn.append(Current_y)\n",
    "                Vec_time_turn.append(Total_step)\n",
    "            Current_x=Next_x\n",
    "            Current_y=Next_y\n",
    "            Current_angle=Next_angle\n",
    "            Total_step+=1\n",
    "Run()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "im_movement=Movement_plot(Vec_x_tripod,Vec_y_tripod,Vec_x_quadruped,Vec_y_quadruped,Vec_x_onebyone,Vec_y_onebyone,\n",
    "                  Vec_x_turn,Vec_y_turn,1500,250)\n",
    "plt.imshow(im_movement)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Vec_time"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
